//
// Created by s on 2025/10/13.
//

#ifndef COREXY_KINEMATICS_KINODYNAMIC_H
#define COREXY_KINEMATICS_KINODYNAMIC_H

#include "../Kinematics/kinematics.h"
#include "math.h"

extern const float DIAMETER;

typedef struct MotorCfg_Down
{
    unsigned char direction_a;
    float velocity_a;
    float angle_a;
    float acceleration_a;

    unsigned char direction_b;
    float velocity_b;
    float angle_b;
    float acceleration_b;
} MotorCfg_Down;

MotorCfg_Down PlanMotorMove(MotorCfg_Up *cfg_up, float tool_distance);

float cordi_2_distance(Coordinate cordi_now, Coordinate cordi_target);
#endif //COREXY_KINEMATICS_KINODYNAMIC_H
